from global_var import *
from All_Init import *
import global_var
from PID import *
import math
from machine import *
M_PI = 3.1415926

uart4 = UART(3)
uart4.init(460800)
ccd1_ave_threshold = ccd2_ave_threshold = ccd3_ave_threshold = 1000
ccd1_mid = ccd2_mid = ccd3_mid = 64
ccd_error = ccd1_error = ccd2_error = ccd3_error = 0
ccd1_error_last = ccd2_error_last = ccd3_error_last = 0
ccd1_center = ccd2_center = ccd3_center = 64
ccd1_center_last = ccd2_center_last = ccd3_center_last = 64
ac_ccd1_road_len = ccd1_road_len = 48
ac_ccd2_road_len = ccd2_road_len = 48
ac_ccd3_road_len = ccd3_road_len = 54
ccd1_left_index = ccd2_left_index = ccd3_left_index = 0
ccd1_right_index = ccd2_right_index = ccd3_right_index = 0
ccd1_left_index_last = ccd2_left_index_last = ccd3_left_index_last = 0
ccd1_right_index_last = ccd2_right_index_last = ccd3_right_index_last = 0
ccd2_left_lose_flag = ccd2_right_lose_flag = 0
def get_the_dealCCd(ccd_data1, ccd_data2, ccd_data3):
    global ccd1_ave_threshold, ccd2_ave_threshold, ccd3_ave_threshold
    ccd1_ave_threshold = sum(ccd_data1) // 128
    ccd2_ave_threshold = sum(ccd_data2) // 128
    ccd3_ave_threshold = sum(ccd_data3) // 128
def cbh_ccd_thr(ccd_data, ccd_num):
    global c1_cbh_thr, c2_cbh_thr, c3_cbh_thr
    ccd_max = max(ccd_data[5:122])
    ccd_min = min(ccd_data[5:122])
    cbh_thr = abs(ccd_max - ccd_min) * 100 // (ccd_max + ccd_min + 1) * getattr(global_var, f"c{ccd_num}_cbh")
    if ccd_num == 1:
        c1_cbh_thr = cbh_thr
    elif ccd_num == 2:
        c2_cbh_thr = cbh_thr
    elif ccd_num == 3:
        c3_cbh_thr = cbh_thr
ccd1_search_left_flag = 0
ccd1_search_right_flag = 0
ccd1_right_bounary = 84
ccd1_left_bounary = 44
ccd1_right_end = 115
ccd1_left_end = 12
ccd1_search_wide = 5
left_start = 84 - 8
left_end = 12
right_start = 44 + 8
right_end = 115
def find_the_ccd1Center(ccd_data1,ccd1_center_last,ccd1_left_index,ccd1_right_index):
    global ccd1_center
    global ac_ccd1_road_len
    global ccd1_road_len
    global ccd1_left_index_last
    global ccd1_right_index_last
    global ccd1_mid
    global ccd1_ave_threshold
    global c1_cbh_thr
    global ccd1_search_left_flag,ccd1_search_right_flag
    global ccd1_search_wide
    global ccd1_right_bounary,ccd1_left_bounary
    global ccd1_right_end,ccd1_left_end
    global left_start, left_end, right_start, right_end
    ccd1_search_left_flag = 0
    ccd1_search_right_flag = 0
    ccd1_center_last = ccd1_center
    right_start = max(right_start, ccd1_left_end)
    right_end = min(right_end, ccd1_right_end)
    left_start = min(left_start, ccd1_right_end)
    left_end = max(left_end, ccd1_left_end)
    for i in range(left_start,left_end,-1):
        end = i - 3
        if(end < ccd1_left_end):
            end = ccd1_left_end
        cbh = (ccd_data1[i] - ccd_data1[end] )*100 //(ccd_data1[i] + ccd_data1[end] + 1)
        if((cbh > c1_cbh_thr) or (ccd_data1[i] - ccd_data1[end] > ccd1_ave_threshold * global_var.c1_avth)):
            ccd1_left_index = end
            ccd1_search_left_flag = 1
            break
        elif(ccd1_search_left_flag == 0 and end == ccd1_left_end):
            ccd1_left_index = end
            break
    for i in range(right_start,right_end,1):
        end = i + 3
        if(end > ccd1_right_end):
            end = ccd1_right_end
        cbh = (ccd_data1[i] - ccd_data1[end] )*100 //(ccd_data1[i] + ccd_data1[end] + 1)
        if((cbh > c1_cbh_thr) or (ccd_data1[i] - ccd_data1[end] > ccd1_ave_threshold * global_var.c1_avth) ):
            ccd1_right_index = end
            ccd1_search_right_flag = 1
            break
        elif(ccd1_search_right_flag == 0 and end == ccd1_right_end):
            ccd1_right_index = end
            break
    left_start = ccd1_center_last + ccd1_search_wide
    left_end = ccd1_left_end
    right_start = ccd1_center_last - ccd1_search_wide
    right_end = ccd1_right_end
    ac_ccd1_road_len = ccd1_right_index - ccd1_left_index
    ccd1_center = (ccd1_left_index + ccd1_right_index) // 2
    return ccd1_center,ccd1_center_last,ccd1_left_index,ccd1_right_index,ccd1_left_index_last,ccd1_right_index_last,ac_ccd1_road_len

ccd2_search_left_flag = 0
ccd2_search_right_flag = 0
ccd2_right_bounary = 88
ccd2_left_bounary = 40
ccd2_right_end = 123
ccd2_left_end = 4
ccd2_search_wide = 10
c2_left_start = 88 - 8
c2_left_end = 4
c2_right_start = 40 + 8
c2_right_end = 123
def find_the_ccd2Center(ccd_data2,ccd2_center_last,ccd2_left_index,ccd2_right_index):
    global ccd2_road_len,ac_ccd2_road_len,ccd2_center
    global ccd2_left_index_last,ccd2_right_index_last,ccd2_mid
    global ccd2_left_lose_flag,ccd2_right_lose_flag
    global ccd2_ave_threshold,c2_cbh_thr,ccd2_search_left_flag,ccd2_search_right_flag
    global ccd2_search_wide,ccd2_right_bounary,ccd2_left_bounary,ccd2_right_end,ccd2_left_end
    global c2_left_start,c2_left_end,c2_right_start,c2_right_end,Left_Island_flag,Right_Island_flag,Island_State,Ramp_flag
    ccd2_search_left_flag = 0
    ccd2_search_right_flag = 0
    ccd2_left_lose_flag = 0
    ccd2_right_lose_flag = 0
    ccd2_center_last = ccd2_center
    c2_right_start = max(c2_right_start, ccd2_left_end)
    c2_right_end = min(c2_right_end, ccd2_right_end)
    c2_left_start = min(c2_left_start, ccd2_right_end)
    c2_left_end = max(c2_left_end, ccd2_left_end)
    for i in range(c2_left_start,c2_left_end,-1):
        end = i - 3
        if(end < ccd2_left_end):
            end = ccd2_left_end
        cbh = (ccd_data2[i] - ccd_data2[end] )*100 //(ccd_data2[i] + ccd_data2[end] + 1)
        if(((cbh > c2_cbh_thr) or (ccd_data2[i] - ccd_data2[end] > ccd2_ave_threshold * global_var.c2_avth))):
            ccd2_left_index = end
            ccd2_search_left_flag = 1
            break
        elif(ccd2_search_left_flag == 0 and end == c2_left_end):
            ccd2_left_index = end
            break
    for i in range(c2_right_start,c2_right_end,1):
        end = i + 3
        if(end > ccd2_right_end):
            end = ccd2_right_end
        cbh = (ccd_data2[i] - ccd_data2[end] )*100 //(ccd_data2[i] + ccd_data2[end] + 1)
        if(((cbh > c2_cbh_thr) or (ccd_data2[i] - ccd_data2[end] > ccd2_ave_threshold * global_var.c2_avth))):
            ccd2_right_index = end
            ccd2_search_right_flag = 1
            break
        elif(ccd2_search_right_flag == 0 and end == c2_right_end):
            ccd2_right_index = end
            break
    c2_left_start = ccd2_center_last + ccd2_search_wide
    c2_left_end = ccd2_left_end
    c2_right_start = ccd2_center_last - ccd2_search_wide
    c2_right_end = ccd2_right_end
    if (Left_Island_flag == 1 and Island_State == 3 ):
        c2_left_start = max(c2_left_start,30)
    if (Right_Island_flag == 1 and Island_State == 3 ):
        c2_right_start = max(c2_right_start,90)
    ac_ccd2_road_len = ccd2_right_index - ccd2_left_index
    ccd2_center = (ccd2_left_index + ccd2_right_index) // 2
    return ccd2_center,ccd2_center_last,ccd2_left_index,ccd2_right_index,ccd2_left_index_last,ccd2_right_index_last,ac_ccd2_road_len

ccd3_search_left_flag = 0
ccd3_search_right_flag = 0
ccd3_right_bounary = 92
ccd3_left_bounary = 36
ccd3_right_end = 123
ccd3_left_end = 4
ccd3_search_wide = 3
c3_left_start = 92 - 8
c3_left_end = 4
c3_right_start = 36 + 8
c3_right_end = 123
def find_the_ccd3Center(ccd_data3,ccd3_center_last,ccd3_left_index,ccd3_right_index):
    global ccd3_road_len,ac_ccd3_road_len,ccd3_center,ccd2_center
    global ccd3_left_index_last,ccd3_right_index_last,ccd3_mid,ccd3_ave_threshold,c3_cbh_thr
    global ccd3_search_left_flag,ccd3_search_right_flag,ccd3_search_wide,ccd3_right_bounary,ccd3_left_bounary
    global ccd3_right_end,ccd3_left_end,c3_left_start,c3_left_end,c3_right_start,c3_right_end
    global Ramp_flag
    ccd3_search_left_flag = 0
    ccd3_search_right_flag = 0
    ccd3_center_last = ccd3_center
    if(Ramp_flag != 0):
        c3_left_start = ccd3_right_bounary
        c3_right_start = ccd3_left_bounary
    c3_right_start = max(c3_right_start, ccd3_left_end)
    c3_right_end = min(c3_right_end, ccd3_right_end)
    c3_left_start = min(c3_left_start, ccd3_right_end)
    c3_left_end = max(c3_left_end, ccd3_left_end)
    for i in range(c3_left_start,c3_left_end,-1):
        end = i - 3
        if(end < ccd3_left_end):
            end = ccd3_left_end
        cbh = (ccd_data3[i] - ccd_data3[end] )*100 //(ccd_data3[i] + ccd_data3[end] + 1)
        if((cbh > c3_cbh_thr) or (ccd_data3[i] - ccd_data3[end] > ccd3_ave_threshold * global_var.c3_avth)):
            ccd3_left_index = end
            ccd3_search_left_flag = 1
            break
        elif(ccd3_search_left_flag == 0 and end == c3_left_end):
            ccd3_left_index = end
            break
    for i in range(c3_right_start,c3_right_end,1):
        end = i + 3
        if(end > ccd3_right_end):
            end = ccd3_right_end
        cbh = (ccd_data3[i] - ccd_data3[end] )*100 //(ccd_data3[i] + ccd_data3[end] + 1)
        if((cbh > c3_cbh_thr)or (ccd_data3[i] - ccd_data3[end] > ccd3_ave_threshold * global_var.c3_avth)):
            ccd3_right_index = end
            ccd3_search_right_flag = 1
            break
        elif(ccd3_search_right_flag == 0 and end == c3_right_end):
            ccd3_right_index = end
            break
    c3_left_start = ccd3_center_last + ccd3_search_wide
    c3_left_end = ccd3_left_end
    c3_right_start = ccd3_center_last - ccd3_search_wide
    c3_right_end = ccd3_right_end
    ac_ccd3_road_len = ccd3_right_index - ccd3_left_index
    ccd3_center = (ccd3_left_index + ccd3_right_index) // 2
    return ccd3_center,ccd3_center_last,ccd3_left_index,ccd3_right_index,ccd3_left_index_last,ccd3_right_index_last,ac_ccd3_road_len
Cross_flag = 0
Cross_State = 0
def Cross_Detect():
    global ccd1_left_index,ccd1_right_index,ccd2_left_index,ccd2_right_index,ccd2_center,ccd2_mid
    global ac_ccd1_road_len,ac_ccd2_road_len
    global Cross_State,Cross_flag,Island_State,ccd1_ave_threshold,ccd2_ave_threshold
    global ccd1_search_left_flag,ccd1_search_right_flag,ccd2_search_left_flag,ccd2_search_right_flag
    global ccd2_left_lose_flag,ccd2_right_lose_flag,Cross_lose_flag
    if(ac_ccd1_road_len > 98 and Island_State == 0 and ccd1_ave_threshold >= int(global_var.Param_dict["c1_ave"]) and Cross_flag == 0):
        if(ac_ccd2_road_len - ccd2_road_len >= 20):
            if(30 <= ccd2_left_index <= 60 and ccd2_right_index >= 110):
                ccd2_center = ccd2_left_index + int(global_var.Param_dict["cro2L"])
                ccd2_right_lose_flag = 1
            elif(70 <= ccd2_right_index <= 105 and ccd2_left_index <= 20):
                ccd2_center = ccd2_right_index + int(global_var.Param_dict["cro2R"])
                ccd2_left_lose_flag = 1
        if(ac_ccd2_road_len >= 110):
            ccd2_center = ccd2_mid
            ccd2_left_lose_flag = 1
            ccd2_right_lose_flag = 1
            Cross_lose_flag = 1
    if (((ccd2_left_index <= 18 and ccd2_right_index >= 112)) and Island_State == 0 and ccd2_ave_threshold >= int(global_var.Param_dict["c2_ave"])):
        if((ccd1_left_index >= 35 and ccd1_right_index <= 92) or (ccd1_left_index <= 20 and ccd1_right_index >= 110)):
            Cross_State = 1
            Cross_flag = 1
        elif ((25 <= ccd1_left_index <= 55) and ccd1_search_right_flag == 0):#右
            Cross_State = 2
            Cross_flag = 1
        elif (ccd1_search_left_flag == 0 and (85 <= ccd1_right_index <= 108)):#左
            Cross_State = 3
            Cross_flag = 1
    elif (ccd2_search_left_flag == 1 and ccd2_search_right_flag == 1):
       Cross_flag = 0
       Cross_State = 0
def Cross_Task():
    global ccd1_left_index,ccd1_right_index
    global ccd3_left_index,ccd3_right_index
    global ccd1_center,ccd3_center
    global Cross_State,Cross_flag
    if Cross_flag == 1:
        if Cross_State == 1 :
            ccd1_center = ccd1_center
        elif Cross_State == 2 :
            ccd1_center = ccd1_left_index + int(global_var.Param_dict["cro1L"])
        elif Cross_State == 3 :
            ccd1_center = ccd1_right_index + int(global_var.Param_dict["cro1R"])
Island_State = 0
Island_Judge = 0
Right_Island_flag = 0
Left_Island_flag = 0
count = 0
check_flag = 0
def Island_Detect():
    global ccd1_right_index ,ccd2_right_index ,ccd3_right_index,ccd1_left_index, ccd2_left_index, ccd3_left_index
    global ccd1_left_index_last, ccd2_left_index_last,ccd3_left_index_last,ccd1_right_index_last,ccd2_right_index_last,ccd3_right_index_last
    global ac_ccd2_road_len,ac_ccd3_road_len,ccd2_road_len
    global Island_State,Right_Island_flag,Left_Island_flag,check_flag,Island_Judge,check_flag
    global ccd1_ave_threshold,ccd2_ave_threshold,red_flag,count
    global ccd2_search_left_flag,ccd2_search_right_flag
    if(red_flag == 1 and ccd2_search_left_flag == 1 and ccd2_search_right_flag == 1 and Island_State == 0 and (int(global_var.Param_dict["L_Is"]) == 1 or int(global_var.Param_dict["R_Is"]) == 1)):
        Island_Judge = 1 
        check_flag = 0
    if(Island_Judge == 1 and Island_State == 0 and int(global_var.Param_dict["L_Is"]) == 1 and
       (((ccd2_left_index - ccd2_left_index_last <= -15) and (80 <= ccd2_right_index <= 105))
       )):
        Left_Island_flag = 1
        Island_State = 1
        Island_Judge = 0
    elif(Island_Judge == 1 and Island_State == 0 and int(global_var.Param_dict["R_Is"]) == 1 and
         (((ccd2_right_index - ccd2_right_index_last >= 15) and (21 <= ccd2_left_index <= 55))
#            or (40<=ccd2_left_index<=60 and ccd2_right_index>=110)
         )):
        Right_Island_flag = 1
        Island_State = 1
        Island_Judge = 0
    if (Left_Island_flag == 1):
        if(Island_State == 1):
            if(ccd2_left_index <= 10 and check_flag == 0):
                Island_State = 2
        elif(Island_State == 2):
            if(ccd2_left_index >= 35):
                check_flag = 1
            if(ccd2_left_index <= 20 and check_flag == 1):
                Island_State = 3
                check_flag = 0
        elif(Island_State == 3):
            if(ccd2_right_index >= 108):
                check_flag = 1
            if(ccd2_right_index <= 90 and check_flag == 1):
                Island_State = 4
                check_flag = 0
        elif(Island_State == 4):
            count += 1
            if(ac_ccd2_road_len > 100 and ccd2_search_right_flag == 0 and ccd2_ave_threshold >= int(global_var.Param_dict["c2_ave"]) and count >= 50):
                Island_State = 5
                count = 0
        elif(Island_State == 5):
            if((ccd2_right_index - ccd2_right_index_last <= -1)):
                count += 1
            elif((ccd2_right_index - ccd2_right_index_last >= 0) and count >= 3):
                Island_State = 6
                count = 0
            else:
                count = 0
        elif(Island_State == 6):
            if(ccd2_search_left_flag == 0):
                check_flag = 1
            if(65 <= ccd2_right_index <= 100 and 30 <= ccd2_left_index <= 50 and check_flag == 1):#ccd2_search_left_flag == 1
                Island_State = 0
                Left_Island_flag = 0
                check_flag = 0
    elif (Right_Island_flag == 1):
        if(Island_State == 1):
            if(ccd2_right_index >= 117 and check_flag == 0):
                Island_State = 2
        elif(Island_State == 2):
            if(ccd2_right_index <= 102):
                check_flag = 1
            if(ccd2_right_index >= 106 and check_flag == 1):
                Island_State = 3
                check_flag = 0
        elif(Island_State == 3):
            if(ccd2_left_index <= 20):
                check_flag = 1
            if(ccd2_left_index >= 40 and check_flag == 1):
                Island_State = 4
                check_flag = 0
        elif(Island_State == 4):
            count += 1
            if(ac_ccd2_road_len > 100 and ccd2_search_left_flag == 0 and ccd2_ave_threshold >= int(global_var.Param_dict["c2_ave"]) and count >= 50):
                Island_State = 5
                count = 0
        elif(Island_State == 5):
            if((ccd2_left_index - ccd2_left_index_last >= 1)):
                count += 1
            elif((ccd2_left_index - ccd2_left_index_last <= 0) and count >= 3):
                Island_State = 6
                count = 0
            else:
                count = 0
        elif(Island_State == 6):
            if(ccd2_search_right_flag == 0 ):
                check_flag = 1
            if(ccd2_search_left_flag == 1 and 80 <= ccd2_right_index <= 110 and check_flag == 1):
                Island_State = 0
                Right_Island_flag = 0
                check_flag = 0
def Island_Task():  
    global Right_Island_flag,Left_Island_flag,Island_State
    global ccd2_left_index,ccd2_right_index
    global ccd3_left_index,ccd3_right_index
    global ccd2_center,ccd3_center
    if Left_Island_flag == 1:#左
        if Island_State == 1:
            ccd2_center = ccd2_right_index + int(global_var.Param_dict["LIs1"])
        elif Island_State == 2:
            ccd2_center = ccd2_right_index + int(global_var.Param_dict["LIs2"])
        elif Island_State == 3:
            ccd2_center = ccd2_left_index + int(global_var.Param_dict["LIs3"])
        elif Island_State == 4:
            ccd2_center = ccd2_center + int(global_var.Param_dict["LIs4"])
        elif Island_State == 5:
            ccd2_center = ccd2_left_index + int(global_var.Param_dict["LIs5"])
        elif Island_State == 6:
            ccd2_center = ccd2_right_index + int(global_var.Param_dict["LIs6"])
    elif Right_Island_flag == 1 :#右
        if Island_State == 1:
            ccd2_center = ccd2_left_index + int(global_var.Param_dict["RIs1"])
        elif Island_State == 2:
            ccd2_center = ccd2_left_index + int(global_var.Param_dict["RIs2"])
        elif Island_State == 3:
            ccd2_center = ccd2_right_index + int(global_var.Param_dict["RIs3"])
        elif Island_State == 4:
            ccd2_center = ccd2_center + int(global_var.Param_dict["RIs4"])
        elif Island_State == 5:
            ccd2_center = ccd2_right_index + int(global_var.Param_dict["RIs5"])
        elif Island_State == 6:
            ccd2_center = ccd2_left_index + int(global_var.Param_dict["RIs6"])
Ramp_State = 0
Ramp_flag = 0
Ramp_count = 0
Ramp_done = False
green_mode = 0
green_count = 0
green_task = 0
Ramp_task_flag = 0
def Ramp_Detect():
    global green_flag,Ramp_State,Ramp_flag,Zebra_State
    global Ramp_done,green_mode,green_count,green_task,Ramp_task_flag
    if (green_flag == 1 and int(global_var.Param_dict["Rmp"] != 0) and abs(ccd3_error) <= 8 and
        not Ramp_done and (Zebra_State == 3 or int(global_var.Param_dict["check"] != 0))):
        Ramp_State = 1
        Ramp_flag = 1
        green_mode = 0
    if(green_flag == 1 and (int(global_var.Param_dict["Rmp"]) == 0 or Ramp_done)):
        Ramp_task_flag = 1
    if Ramp_task_flag == 1:
        if green_mode == 0:
            green_count+=1
            green_task = 1
            if(green_count>global_var.Param_dict["G_cot1"]):
                green_count =0
                green_mode = 1
                green_task = 0
                Ramp_task_flag = 0
        elif green_mode == 1:
            green_count+=1
            green_task = 2
            if(green_count>global_var.Param_dict["G_cot2"]):
                green_count =0
                green_mode = 3
                green_task = 0
                Ramp_task = 0
    
Ramp_check = 0
Ramp_finish_count = 0
Ramp_finish_flag = 0
Ramp_slow_flag = 0
def Ramp_Task():
    global Ramp_State,Ramp_flag,Ramp_count,Ramp_check,Ramp_finish_count,Ramp_finish_flag,Ramp_slow_flag,Ramp_done
    global ac_ccd3_road_len
    if Ramp_flag == 1:
        Ramp_count +=1
        if Ramp_State == 1:
            if ac_ccd3_road_len >= 110 and Ramp_count >= 15:
                Ramp_State = 2
                Ramp_check = 0
        elif Ramp_State == 2:
            if ac_ccd3_road_len <= 45:
                Ramp_State = 3
        elif Ramp_State == 3:
            if (global_var.Gyro['az'] > 15000):
                Ramp_finish_flag = 1
            if(Ramp_finish_flag == 1):
                Ramp_finish_count += 1
            if(Ramp_finish_count >= 20):
                Ramp_slow_flag = 1
            if(Ramp_finish_count >= 60):
                Ramp_State = Ramp_flag = Ramp_count = Ramp_finish_count = Ramp_finish_flag = Ramp_slow_flag = 0
                Ramp_done = True
        if(Ramp_count >= int(global_var.Param_dict["Rmp_cot"])):
            Ramp_State = Ramp_flag = Ramp_count = Ramp_finish_count = Ramp_finish_flag = Ramp_slow_flag = 0
            Ramp_done = True
Zebra_State = Zebra_Count = Block_num_1 = Block_num_3 = Zebra_3_flag = 0
def Zebra_Detect(ccd_data1,ccd_data3):
    global Zebra_State, Zebra_Count, Block_num_1,Block_num_3,Zebra_3_flag,current_detected_3,Ramp_State
    Block_num_1 = 0
    Block_num_3 = 0
    current_detected_1 = 0
    current_detected_3 = 0
    for i in range(30, 98):
        cbh1 = abs(ccd_data1[i] - ccd_data1[i + 1]) * 100 / (ccd_data1[i] + ccd_data1[i + 1] + 1)
        cbh3  = abs(ccd_data3[i] - ccd_data3[i + 1]) * 100 / (ccd_data3[i] + ccd_data3[i + 1] + 1)
        if (cbh1 > 10):
            Block_num_1 += 1
        if (cbh3 > 10):
            Block_num_3 += 1
    if(Ramp_State == 0):
        current_detected_1 = 1 if Block_num_1 >= 15 else 0
        current_detected_3 = 1 if Block_num_3 >= 15 else 0
    if current_detected_1:
        if Zebra_State == 0:
            Zebra_State = 1
        elif Zebra_State == 3:
            Zebra_State = 4
    else:
        if Zebra_State == 1:
            Zebra_State = 2
    if current_detected_3 and (Zebra_State == 2 or Zebra_State == 4):
        Zebra_3_flag = 1
def Zebra_Task():
    global Zebra_State,Zebra_Count,current_detected_3,Zebra_3_flag
    if Zebra_State == 2 and Zebra_3_flag == 1:
        if current_detected_3 == 0:
            Zebra_State = 3
            Zebra_3_flag = 0
    if Zebra_State == 4 and Zebra_3_flag == 1:
        if current_detected_3 == 0:
            Zebra_State = 5
            Zebra_3_flag = 0
    if Zebra_State == 5:
        global_var.Stop_start = 1
        Zebra_State = 0
        Zebra_Count = 0
L_block_flag = 0
R_block_flag = 0
block_state = 0
block_check = 0
block_check_count = 0
block_num = 0
block_count = 0 
def block_Detect():
    global ccd1_left_index,ccd1_right_index,ac_ccd1_road_len,ccd1_left_index_last,ccd1_right_index_last,ccd1_center
    global ccd2_left_index,ccd2_right_index,ac_ccd2_road_len,ccd2_left_index_last,ccd2_right_index_last
    global ccd3_left_index,ccd3_right_index,ac_ccd3_road_len
    global L_block_flag,R_block_flag,block_state,block_num,block_count,block_check,block_check_count
    global Island_State,Ramp_State,Zebra_State
    if(ac_ccd1_road_len <= 45 and (35 <= ccd2_left_index <= 60 and 80 <= ccd2_right_index <= 100)
       and block_state == 0 and Island_State == 0 and Ramp_State == 0 and (Zebra_State == 3 or int(global_var.Param_dict["check"] != 0))
       and int(global_var.Param_dict["block"]) != 0 and (block_num < int(global_var.Param_dict["blknum"]))):
        if ((50 <= ccd1_left_index <= 64) and (78 < ccd1_right_index <= 95) and (ccd1_left_index - ccd1_left_index_last >= 8) or
            (68 <= ccd1_right_index <= 84) and (35 <= ccd1_left_index <= 58) and (ccd1_right_index - ccd1_right_index_last <= -8)):
            block_check = 1
    if block_check == 1:
        block_check_count += 1
        if(block_check_count <= 8):
            if(ccd2_left_index - ccd2_left_index_last >= 8):
                L_block_flag = 1
                block_state = 1
            if(ccd2_right_index - ccd2_right_index_last <= -8):
                R_block_flag = 1
                block_state = 1
        else:
            block_check = 0
            block_check_count = 0
    if(block_state >= 1):
        block_count += 1
        block_check = 0
        block_check_count = 0
    if(block_state == 1)  and (ac_ccd2_road_len <= 42):
        block_state = 2
    if(block_state == 2) and (((ccd3_left_index >= 30) and L_block_flag == 1) or ((ccd3_right_index < 102) and R_block_flag == 1)):
        block_state = 3
    if(((block_state == 3) and (((ccd3_left_index <= 25) and L_block_flag == 1) or ((ccd3_right_index >= 115)) and R_block_flag == 1)) or (block_count >= int(global_var.Param_dict["blkcot"]))):
        L_block_flag = R_block_flag = block_state = block_count = 0
        block_num += 1
def block_Task():
    global L_block_flag,R_block_flag,block_state
    global ccd1_center,ccd1_left_index,ccd1_right_index
    if L_block_flag == 1:
        ccd1_center = ccd1_right_index - int(global_var.Param_dict["blk_bu"])
    elif R_block_flag == 1:
        ccd1_center = ccd1_left_index + int(global_var.Param_dict["blk_bu"])

Slow_flag = 0
Slow_State = 0
Slow_count = 0
Slow_finish_flag = 0
Slow_check = 0
def Slow_Detect():
    global yellow_flag,Slow_flag,Slow_count,Slow_State,ccd_error,Slow_finish_flag,Slow_check
    if(yellow_flag == 1 and int(global_var.Param_dict["Slow"]) != 0 and Slow_flag == 0):
        Slow_flag = 1
        Slow_State = 1
    if Slow_flag == 1:
        Slow_count += 1
        if Slow_count >= int(global_var.Param_dict["Slocot1"]):
            Slow_State = 2
            if abs(ccd_error) >= 15:
                Slow_check = 1
            if Slow_check == 1 and abs(ccd_error) < 6:
                Slow_check = 0
                Slow_finish_flag = 1
        if Slow_count >= int(global_var.Param_dict["Slocot2"]) or Slow_finish_flag == 1:
            Slow_State = 0
            Slow_flag = 0
            Slow_count = 0
            Slow_check = 0
            Slow_finish_flag = 0

red_flag = 0
green_flag = 0
yellow_flag = 0
buffer_cleared = False
def Openart_Detect():
    global red_flag,green_flag,yellow_flag
    global Island_State,Ramp_State,Slow_flag
    global buffer_cleared
    red_flag = 0
    green_flag = 0
    yellow_flag = 0
    if not buffer_cleared:
        if uart4.any():
            uart4.read(uart4.any())
        time.sleep_ms(50)
        if uart4.any():
            uart4.read(uart4.any())
        buffer_cleared = True
    buf_len = uart4.any()
    if(buf_len):
        raw_buf = uart4.read(buf_len).lower()
        if (red_flag == 0 and Island_State == 0 and ("red" in raw_buf)):
            red_flag = 1
        if(green_flag == 0 and Ramp_State == 0 and ("green" in raw_buf)):
            green_flag = 1
        if(yellow_flag == 0 and Slow_flag == 0 and ("yellow" in raw_buf)):
            yellow_flag = 1
"""误差处理"""
def direction_control():
    global ccd1_road_len,ccd2_road_len,ccd3_road_len
    global ccd1_center,ccd2_center,ccd3_center
    global ccd1_mid,ccd2_mid,ccd3_mid
    global ccd1_error,ccd2_error,ccd3_error
    global ccd1_error_last,ccd2_error_last,ccd3_error_last
    global ccd1_center_last,ccd2_center_last,ccd3_center_last
    ccd1_center_last = ccd1_center
    ccd2_center_last = ccd2_center
    ccd3_center_last = ccd3_center
    ccd1_error_last = ccd1_error
    ccd2_error_last = ccd2_error
    ccd3_error_last = ccd3_error
    ccd1_error = ccd1_center - ccd1_mid
    ccd2_error = ccd2_center - ccd2_mid
    ccd3_error = ccd3_center - ccd3_mid
    return ccd1_error,ccd2_error,ccd3_error

def Cal_Err():
    global ccd1_error,ccd2_error,ccd3_error,ccd_error
    global Cross_flag,Cross_State,Island_State,Zebra_State,Ramp_State,Ramp_finish_flag,Slow_flag
    global Right_Island_flag,Left_Island_flag,Island_Detect_flag
    global ccd1_search_left_flag,ccd1_search_right_flag
    global ccd2_search_left_flag,ccd2_search_right_flag
    global ccd1_ave_threshold,ccd2_ave_threshold
    ccd_error = ccd2_error
    if Cross_flag == 1 and (Cross_State == 1 or Cross_State == 2 or Cross_State == 3):
        ccd_error = ccd1_error
    if Ramp_State != 0 :
        ccd_error = ccd3_error
        if Ramp_finish_flag != 0:
            ccd_error = ccd2_error
        ccd_error = max(min(ccd_error,10),-10)
    if(4 <= Island_State <= 5):
        if Island_State == 4:
            if Left_Island_flag == 1:
                ccd_error = max(min(ccd2_error,-12),-30)
            if Right_Island_flag == 1:
                ccd_error = min(max(ccd2_error,12), 27)
        if Island_State == 5:
            if Left_Island_flag == 1:
                ccd_error = max(min(ccd2_error,-15),-24)
            if Right_Island_flag == 1:
                ccd_error = min(max(ccd2_error,15),26)
    if Zebra_State != 0:
        if (1 <= Zebra_State <= 2) or (4 <= Zebra_State <= 5):
            ccd_error = min([ccd1_error,ccd2_error,ccd3_error],key=abs)
    if block_state != 0:
        ccd_error = ccd1_error
    return ccd_error
aim_speed = 0
def Target_Select(Real_speed):
    global aim_speed,Island_Judge,Island_State,Zebra_State,Slow_State
    global Straight_flag,Left_Island_flag,Right_Island_flag,Ramp_State
    if Island_State != 0:
        global_var.L_dif = global_var.R_dif = 0
        if (Left_Island_flag == 1 or Right_Island_flag == 1) and (2 <= Island_State <= 5):
            aim_speed = global_var.Param_dict["Is_sp"]
            Turn_PID.static_p = global_var.Param_dict["Is_Tp"]
            Turn_PID.static_d = global_var.Param_dict["Is_Td"]
            Acc_PID.Kp = global_var.Param_dict["ac_p"]
            Angle_PID.Kp = global_var.Param_dict["an_p"]
    elif Ramp_State != 0:
        Speed_PID.Kp = global_var.Param_dict["Rmp_skp"]
        Turn_PID.static_p = global_var.Param_dict["Rmp_tkp"]
        global_var.L_dif = global_var.R_dif = 0
        if Ramp_State == 1 or Ramp_State == 2:
            aim_speed = global_var.Param_dict["Rmp_sp"]
            global_var.Mid_Angle = global_var.Param_dict["Rmp_mid1"]
        elif Ramp_State == 3:
            global_var.Mid_Angle = global_var.Param_dict["Rmp_mid2"]
    elif green_task != 0:
        if green_task == 1:
            global_var.Mid_Angle = global_var.Param_dict["green1"]
        elif green_task == 2:
            global_var.Mid_Angle = global_var.Param_dict["green2"]
    elif Slow_State == 1:
#         global_var.Mid_Angle = -100
        global_var.Mid_Angle = global_var.Param_dict["Slomid"]
    else:
        aim_speed = global_var.Crvspeed
        Turn_PID.static_p = global_var.Param_dict["st_p"]
        Turn_PID.static_d = global_var.Param_dict["st_d"]
        Speed_PID.Kp = global_var.Param_dict["sKp"]
        global_var.Mid_Angle =  global_var.Param_dict["Mid"]
        Acc_PID.Kp = global_var.Param_dict["AccKp"]
        Angle_PID.Kp = global_var.Param_dict["AnKp"]
def MODE():
    mode_params = {
        1: {  # 预无
            "speed":300,"sKp":-2.5,"Mid":-320,"st_p":256,"st_d":60,"Is_sp":330,"Is_Tp":250,"Slow":1,"mode":0},
        2: {  # 预2
            "speed":330,"sKp":-1.3,"Mid":-440,"st_p":180,"st_d":60,"Is_sp":330,"Is_Tp":220,"Slow":0,"mode":0},
    }
    if global_var.Mode in mode_params:
        global_var.Param_dict.update(mode_params[global_var.Mode])
"""图像总处理"""
def Image_Process(ccd_data1, ccd_data2,ccd_data3,Real_speed):
    global ccd1_center,ccd1_center_last,ac_ccd1_road_len,ccd1_left_index,ccd1_right_index,ccd1_left_index_last,ccd1_right_index_last,ac_ccd1_road_len
    global ccd2_center,ccd2_center_last,ac_ccd2_road_len,ccd2_left_index,ccd2_right_index,ccd2_left_index_last,ccd2_right_index_last,ac_ccd2_road_len
    global ccd3_center,ccd3_center_last,ac_ccd3_road_len,ccd3_left_index,ccd3_right_index,ccd3_left_index_last,ccd3_right_index_last,ac_ccd3_road_len
    global ccd_error,ccd1_error,ccd2_error,ccd3_error
    get_the_dealCCd(ccd_data1,ccd_data2,ccd_data3)
    cbh_ccd_thr(ccd_data1, 1)
    cbh_ccd_thr(ccd_data2, 2)
    cbh_ccd_thr(ccd_data3, 3)
    ccd1_left_last_temp = ccd1_left_index
    ccd2_left_last_temp = ccd2_left_index
    ccd3_left_last_temp = ccd3_left_index
    ccd1_right_last_temp = ccd1_right_index
    ccd2_right_last_temp = ccd2_right_index
    ccd3_right_last_temp = ccd3_right_index
    ccd1_center,ccd1_center_last,ccd1_left_index,ccd1_right_index,ccd1_left_index_last,ccd1_right_index_last,ac_ccd1_road_len = find_the_ccd1Center(ccd_data1,ccd1_center_last,ccd1_left_index,ccd1_right_index)
    ccd2_center,ccd2_center_last,ccd2_left_index,ccd2_right_index,ccd2_left_index_last,ccd2_right_index_last,ac_ccd2_road_len = find_the_ccd2Center(ccd_data2,ccd2_center_last,ccd2_left_index,ccd2_right_index)
    ccd3_center,ccd3_center_last,ccd3_left_index,ccd3_right_index,ccd3_left_index_last,ccd3_right_index_last,ac_ccd3_road_len = find_the_ccd3Center(ccd_data3,ccd3_center_last,ccd3_left_index,ccd3_right_index)
    Openart_Detect()
    Island_Detect()
    Island_Task()
    Cross_Detect()
    Cross_Task()
    Ramp_Detect()
    Ramp_Task() 
    block_Detect()
    block_Task()
    if(int(global_var.Param_dict["Zebra"]) != 0 and global_var.Car_Go == 1):
        Zebra_Detect(ccd_data1,ccd_data3)
        Zebra_Task()
    Slow_Detect()
    ccd1_error,ccd2_error,ccd3_error = direction_control()
    ccd_error = Cal_Err()
    Target_Select(Real_speed)
    ccd1_left_index_last = ccd1_left_last_temp
    ccd2_left_index_last = ccd2_left_last_temp
    ccd3_left_index_last = ccd3_left_last_temp
    ccd1_right_index_last = ccd1_right_last_temp
    ccd2_right_index_last = ccd2_right_last_temp
    ccd3_right_index_last = ccd3_right_last_temp
    return ccd_error
"""图像数据显示"""
def Element_Get(opt=None):
    global_var.Normal_Go = 1
    lcd.wave(0,  0, 240, 64, global_var.ccd_data1,max = int(global_var.Param_dict["c1_max"]))
    lcd.wave(0, 64, 240, 64, global_var.ccd_data2,max = int(global_var.Param_dict["c2_max"]))

    if opt == 1:#边线
        lcd.str16(20, 130, f"C1_r:{ccd1_right_index}",0xffff)
        lcd.str16(20, 150, f"C1_l:{ccd1_left_index}",0xffff)
        lcd.str16(20, 170, f"C1_c:{ccd1_center}",0xffff)
        lcd.str16(20, 190, f"C1_err:{ccd1_error}",0xffff)
        lcd.str16(20, 210, f"C1_road:{ac_ccd1_road_len}",0xffff)
        lcd.str16(20, 230, f"C1_ser_l:{ccd1_search_left_flag}",0xffff)
        lcd.str16(20, 250, f"C1_ser_r:{ccd1_search_right_flag}",0xffff)
        lcd.str16(20, 270, f"err:{ccd_error:.2f}",0xffff)
        
        lcd.str16(120, 130, f"C2_r:{ccd2_right_index}",0xffff)
        lcd.str16(120, 150, f"C2_l:{ccd2_left_index}",0xffff)
        lcd.str16(120, 170, f"C2_c:{ccd2_center}",0xffff)
        lcd.str16(120, 190, f"C2_err:{ccd2_error}",0xffff)
        lcd.str16(120, 210, f"C2_road:{ac_ccd2_road_len}",0xffff)
        lcd.str16(120, 230, f"C2_los_l:{ccd2_left_lose_flag}",0xffff)
        lcd.str16(120, 250, f"C2_los_r:{ccd2_right_lose_flag}",0xffff)

    elif opt == 2:#环岛
        lcd.wave(0, 128, 240, 64, global_var.ccd_data3,max = int(global_var.Param_dict["c3_max"]))
        lcd.str16(20, 190, f"C2_l:{ccd2_left_index}",0xffff)
        lcd.str16(20, 210, f"C2_r:{ccd2_right_index}",0xffff)
        lcd.str16(20, 230, f"Is_Sta:{Island_State}",0xffff)
        lcd.str16(20, 250, f"Is_check:{check_flag}",0xffff)
        lcd.str16(20, 270, f"Is_L_fla:{Left_Island_flag}",0xffff)
        lcd.str16(20, 290, f"Is_R_fla:{Right_Island_flag}",0xffff)
        
        lcd.str16(120, 190, f"C1_l:{ccd1_left_index}",0xffff)
        lcd.str16(120, 210, f"C1_r:{ccd1_right_index}",0xffff)
        lcd.str16(120, 230, f"count:{count}",0xffff)
        lcd.str16(120, 270, f"err:{ccd_error:.2f}",0xffff)
        
    elif opt == 3:#十字
        lcd.wave(0, 128, 240, 64, global_var.ccd_data3,max = int(global_var.Param_dict["c3_max"]))
        lcd.str16(20, 190, f"Cross_flag:{Cross_flag}",0xffff)
        lcd.str16(20, 210, f"Cross_State:{Cross_State}",0xffff)
        lcd.str16(20, 230, f"err:{ccd_error:.2f}",0xffff)
        
    elif opt == 4:#坡道
        lcd.wave(0, 128, 240, 64, global_var.ccd_data3,max = int(global_var.Param_dict["c3_max"]))
        lcd.str16(20, 190, f"Ramp_State:{Ramp_State}",0xffff)
        lcd.str16(20, 210, f"Ramp_flag:{Ramp_flag}",0xffff)
        lcd.str16(20, 230, f"err:{ccd_error}",0xffff)
    elif opt == 5:#斑马线
        lcd.wave(0, 128, 240, 64, global_var.ccd_data3,max = int(global_var.Param_dict["c3_max"]))
        lcd.str16(20, 190, f"Zebra_State:{Zebra_State}",0xffff)
        lcd.str16(20, 210, f"Block_num1:{Block_num_1}",0xffff)
        lcd.str16(20, 230, f"Block_num3:{Block_num_3}",0xffff)
        lcd.str16(20, 250, f"Zebra_3_flag:{Zebra_3_flag}",0xffff)
        lcd.str16(20, 270, f"err:{ccd_error}",0xffff)
        
    elif opt == 6:#3ccd
        lcd.wave(0, 128, 240, 64, global_var.ccd_data3,max = int(global_var.Param_dict["c3_max"]))
        lcd.str16(20, 190, f"C3_err:{ccd3_error:.1f}",0xffff)
        lcd.str16(20, 210, f"C3_c:{ccd3_center}",0xffff)
        lcd.str16(20, 230, f"ac2_road:{ac_ccd2_road_len}",0xffff)
        lcd.str16(20, 250, f"ac3_road:{ac_ccd3_road_len}",0xffff)
        lcd.str16(20, 270, f"C3_ser_r:{ccd3_search_left_flag}",0xffff)
        lcd.str16(20, 290, f"C3_ser_l:{ccd3_search_right_flag}",0xffff)

    elif opt == 7:#图像阈值
        lcd.wave(0, 128, 240, 64, global_var.ccd_data3,max = int(global_var.Param_dict["c3_max"]))
        lcd.str16(10, 190, f"c1_cbh_thr:{c1_cbh_thr:.1f}",0xffff)
        lcd.str16(10, 210, f"c2_cbh_thr:{c2_cbh_thr:.1f}",0xffff)
        lcd.str16(10, 230, f"c3_cbh_thr:{c3_cbh_thr:.1f}",0xffff)
        lcd.str16(10, 250, f"c1_ave:{ccd1_ave_threshold:.1f}",0xffff)
        lcd.str16(10, 270, f"c2_ave:{ccd2_ave_threshold:.1f}",0xffff)        
        lcd.str16(10, 290, f"c3_ave:{ccd3_ave_threshold:.1f}",0xffff)
    elif opt == 8:#Openar
        lcd.str16(10, 130, f"red_flag:{red_flag}",0xffff)
        lcd.str16(10, 150, f"green_flag:{green_flag}",0xffff)
        lcd.str16(10, 170, f"yellow_flag:{yellow_flag}",0xffff)
